#!/usr/bin/env python
__builtins__.__openravepy_version__ = '0.7'

from openravepy import *
from numpy import *
import time

RaveInitialize()
RaveLoadPlugin('./pencil')

env=Environment()
env.Load('scenes/myscene.env.xml')
env.SetViewer('qtcoin')

physics = RaveCreatePhysicsEngine(env,'ode')
env.SetPhysicsEngine(physics)

with env:
    env.GetPhysicsEngine().SetGravity(array((0,0,-9.8)))
    robot = env.GetRobots()[0]
    robot.SetController(RaveCreateController(env,'odevelocity'),range(robot.GetDOF()),0)
    env.StopSimulation()
    env.StartSimulation(timestep=0.001)

starttime = time.time()
while True:
    velocities = 8*(random.rand(robot.GetDOF())-0.5)
    print 'velocities: ',velocities
    robot.GetController().SendCommand('setvelocity '+' '.join(str(f) for f in velocities))
    time.sleep(5)

if __name__=='__main__':
    main()



